#include <cmath>
#include <fstream>
#include "sv2_mot.h"
#include <cmath>
#include <fstream>
#include <limits>
#include <vector>


using namespace std;
using namespace Eigen;

namespace sv2
{

  void MultipleObjectTrackerNode::run()
  {
    while (this->is_running())
    {
      nlohmann::json image_json;
      nlohmann::json tgts_json;

      // 主线程阻塞，等待队里中出现数据
      std::unique_lock<std::mutex> lock(this->_full_queue_mtx);
      this->_full_cv.wait(lock, [this]
                          { return !this->_full_queue.empty(); });

      image_json = this->_full_queue.front();
      this->_full_queue.pop();
      tgts_json = image_json["spirecv_msgs::2DTargets"];

      //if ("sort" == this->_algorithm && this->_sort_impl)
      //{
        //this->_sort_impl->update(tgts_json, this->_mot_id);
      //}
      if ("bytetrack" == this->_algorithm && this->_bytetrack_impl)
      {
        this->_bytetrack_impl->update(tgts_json, this->_mot_id);
      }

      image_json["spirecv_msgs::2DTargets"] = tgts_json;
      _res_pub.publish(tgts_json);
      _res_img_pub.publish(image_json);
    }
  }
}
